/**
	@file dhrystone.c
	@author Andrew D. Zonenberg
	@brief Dhrystone benchmark control file
 */
#include <achd-soc.h>
#include <stdarg.h>

void printf(const char* format, ...)	__attribute__ ((section(".startup")));
void print_char(char ch)				__attribute__ ((section(".startup")));
char* itoa(int n, char* s)				__attribute__ ((section(".startup")));
char* reverse(char* s)					__attribute__ ((section(".startup")));
int strlen(const char* str)				__attribute__ ((section(".startup")));
char* strcpy(char* dst, const char* src) __attribute__ ((section(".startup")));
void* memcpy(void* dest, const void* src, unsigned int n) __attribute__ ((section(".startup")));
int strcmp(const char* a, const char* b) __attribute__ ((section(".startup")));

typedef unsigned int clock_t;
struct tms
{
	clock_t tms_utime;  /* user time */
	clock_t tms_stime;  /* system time */
	clock_t tms_cutime; /* user time of children */
	clock_t tms_cstime; /* system time of children */
};
clock_t times(struct tms* buf) __attribute__ ((section(".startup")));

void cp0_timer_init();
int cp0_timer_value();

extern void dhrystone_main();

void main(void)
{
	//80 MHz / 115200 baud
	U1BRG = 695;
	
	//Turn on the LEDs
	LEDBANKA = 0x55;
	
	//Reset the CP0 count register	
	//Doesn't matter when we do this since the actual Dhrystone code takes a delta
	cp0_timer_init();
	
	//Get ready
	//printf("Dhrystone benchmark starting up...\r\n");	
	printf("%d\r\n", g_foobar);
	
	//and run the benchmark
	//dhrystone_main();
		
	while(1);
}

/**
	@brief strcpy() for dhrystone
 */
char* strcpy(char* dst, const char* src)
{
	while(*src)
		*(dst++) = *(src++);
	*dst = 0;
	return dst;
}

void* memcpy(void* dest, const void* src, unsigned int n)
{
	char* d = (char*)dest;
	char* s = (char*) src;
	for(unsigned int i=0; i<n; i++)
		d[i] = s[i];
	return dest;
}

clock_t times(struct tms* buf) 
{
	clock_t now = cp0_timer_value();
	if(buf)
	{
		buf->tms_utime = now;
		buf->tms_stime = 0;
		buf->tms_cutime = 0;
		buf->tms_cstime = 0;
	}
	return now;
}
 
/**
	@brief strcmp() for dhrystone
 */
int strcmp(const char* a, const char* b)
{
	while(*a || *b)
	{
		if( (*a == 0) || (*a > *b) )
			return 1;
		if( (*b == 0) || (*a < *b) )
			return -1;
		a++;
		b++;
	}
	return 0;
}

/**
	@brief Very basic printf function
 */
void printf(const char* format, ...)
{
	va_list list;
	va_start(list, format);
	
	static const char hex[] = "0123456789abcdef";
	static const char Hex[] = "0123456789ABCDEF";
	char numbuf[12];	//= strlen("-2147483648") + room for null
						//largest possible string representation of a 32 bit value
	
	while(*format)
	{
		if(*format == '%')
		{
			//Get the format code
			//Don't support length specifiers for now
			format++;
			switch(*format)
			{
			case 's':
				{
					//It's a string
					const char* s = va_arg(list, const char*);
					while(*s)
					{
						print_char(*s);
						s++;
					}
				}
				break;
				
			case 'd':
				{
					//convert to an int
					itoa(va_arg(list, int), numbuf);
					for(int i=0; i<12; i++)
					{
						if(!numbuf[i])
							break;
						print_char(numbuf[i]);
					}
				}
				break;
				
			case 'x':
			case 'X':
				{
					unsigned int d = va_arg(list, unsigned int);
					int bFirst = 1;
					for(int j=0; j<8; j++)
					{
						//Grab the next 4 bits
						unsigned int x = d >> 28;
						
						//Print it
						char ch = hex[x];
						if(*format == 'X')					//capitalize
							ch = Hex[x];
							
						//Skip leading zeros unless we are padding
						//but print a single 0 if it's zero
						if( (ch == '0') && bFirst && (j != 7) )
						{
						}
						else
						{
							print_char(ch);
							bFirst = 0;
						}	
						
						//Shift off what we just printed
						d <<= 4;
					}	
				}
				break;
			default:
				//unrecognized character, ignore it
				break;
			}
		}
		
		else
			print_char(*format);
		
		format++;
	}	
	
	va_end(list);
}

/**
	@brief Prints a single character
 */
void print_char(char ch)
{
	if(U1STATbits.TXFULL)
	{
		while(!U1STATbits.TXEMPTY)
		{}
	}

	//Send the byte
	U1TXBUF = ch;
}

/*
	@brief Converts a number to a string
 */
char* itoa(int n, char* s)
{
	int i, sign;
	
	if ((sign = n) < 0)  /* record sign */
		n = -n;          /* make n positive */
	i = 0;
	do
	{       /* generate digits in reverse order */
		s[i++] = n % 10 + '0';   /* get next digit */
	} while ((n /= 10) > 0);     /* delete it */
	
	if (sign < 0)
		s[i++] = '-';
	s[i] = '\0';
	
	return reverse(s);
}	

/**
	@brief Reverses a string in-place (K&R implementation)
	
	@param s Input
	@return s after reversing
 */
char* reverse(char* s)
{
	int i, j;
	char c;
	
	for (i = 0, j = strlen(s)-1; i<j; i++, j--)
	{
		c = s[i];
		s[i] = s[j];
		s[j] = c;
	}
	return s;
}

/**
	@brief Returns the length of a null terminated string
 */
int strlen(const char* str)
{
	int i=0;
	while(str[i] != 0)
		i++;
	return i;
}
